import numpy as np
import pandas as pd
dic1 = {
    (1, 1): [0, 45, 90],
    (1, 0): [45, 90, 135],
    (1, -1): [90, 135, 180],
    (0, -1): [135, 180, 225],
    (-1, -1): [180, 225, 270],
    (-1, 0): [225, 270, 315],
    (-1, 1): [270, 315, 0],
    (0, 1): [315, 0, 45],
}
dic2 = {
    (0, (-1, 1)): [270, 315],(0, (0, 1)): [315, 0, 45],(0, (1, 1)): [45, 90],
    (45, (0, 1)): [315, 0],(45, (1, 1)): [0, 45, 90],(45, (1, 0)): [90, 135],
    (90, (1, 1)): [0, 45], (90, (1, 0)): [45, 90, 135], (90, (1, -1)): [135, 180],
    (135, (1, 0)): [45, 90], (135, (1, -1)): [90, 135, 180], (135, (0, -1)): [180, 225],
    (180, (1, -1)): [90, 135], (180, (0, -1)): [135, 180, 225], (180, (-1, -1)): [225, 270],
    (225, (0, -1)): [135, 180], (225, (-1, -1)): [180, 225, 270], (225, (-1, 0)): [270, 315],
    (270, (-1, -1)): [180, 225], (270, (-1, 0)): [225, 270, 315], (270, (-1, 1)): [315, 0],
    (315, (-1, 0)): [225, 270], (315, (-1, 1)): [270, 315, 0], (315, (0, 1)): [0, 45],
}

def read():
    file_path = '附件7：P5-P6行驶路径.xlsx'
    df = pd.read_excel(file_path)
    array = df[['编号','栅格x坐标', '栅格y坐标']].values.tolist()
    return array

path=read()
dp=[{} for _ in range(len(path))]
prev = [{} for _ in range(len(path))]
for i in range(1,len(path)):
    x, y = path[i][1], path[i][2]
    xp, yp = path[i-1][1], path[i-1][2]
    dP =(x-xp,y-yp)
    for j in dic1[dP]:
        dp[i-1][j]=float('inf')
dp[99][0]=float('inf')
dp[99][45]=float('inf')
dp[99][90]=float('inf')
for i in dp[0].keys():
    dp[0][i]=0
for i in range(len(path)-1):
    x, y = path[i][1], path[i][2]
    xn, yn = path[i+1][1], path[i+1][2]
    dP =(xn-x,yn-y)
    for j in dp[i].keys():
        for k in dic2[(j,dP)]:
            if(k in dp[i+1].keys()):
                dtheta=abs(k-j)
                dtheta=min(dtheta,360-dtheta)
                cost = dp[i][j]+ np.sqrt(abs(dP[0])+abs(dP[1]))*5+dtheta/18
                if cost < dp[i+1][k]:
                    dp[i+1][k] = cost
                    prev[i+1][k] = (i,j)

best={0:225,99:45}
aa,bb=98,90
for i in range(98,0,-1):
    best[aa]=bb
    aa,bb=prev[aa][bb]
# 输出最佳路径，包括累积里程
mileage = [dp[i][best[i]] for i in range(len(dp))]
print("点编号\tx坐标\ty坐标\t方向\t累积里程")
for i in range(len(dp)):
    print(f"{i}\t{path[i][1]}\t{path[i][2]}\t{best[i]}\t{mileage[i]}")
for i in dp:
    print(i)